problem.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Fwd refs
namespace wolf
{
class SolverManager;
class HardwareBase;
class TrajectoryBase;
class MapBase;
class ProcessorMotion;
class StateBlock;
class TimeStamp;
struct ParamsSensorBase;
struct ParamsProcessorBase;
}  // namespace wolf
 
// wolf includes
#include "core/common/wolf.h"
#include "core/utils/loader.h"
#include "core/common/node_state_blocks.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
#include "core/composite/prior_composite.h"
#include "core/composite/type_composite.h"
#include "core/composite/vector_composite.h"
#include "core/processor/motion_provider.h"
 
// std includes
#include <mutex>
 
// yaml includes
#include "yaml-cpp/yaml.h"
 
namespace wolf
{
enum Notification
{
    ADD,
    REMOVE
};
 
// Class Problem
class Problem : public std::enable_shared_from_this<Problem>
{
    friend SolverManager;  // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
    friend ProcessorBase;
    friend ProcessorMotion;
    friend MotionProvider;
 
  protected:
    HardwareBasePtr                           hardware_ptr_;
    TrajectoryBasePtr                         trajectory_ptr_;
    MapBasePtr                                map_ptr_;
    TreeManagerBasePtr                        tree_manager_;
    std::map<unsigned int, MotionProviderPtr> motion_provider_map_;
 
    SizeEigen       dim_;
    TypeComposite   frame_types_;
    VectorComposite first_frame_values_;
    PriorComposite  first_frame_priors_;
    unsigned int    first_frame_status_;  
 
    std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
 
    std::map<FactorBasePtr, Notification> factor_notification_map_;
    std::map<StateBlockPtr, Notification> state_block_notification_map_;
    mutable std::mutex                    mut_notifications_;
    mutable std::mutex                    mut_covariances_;
 
    std::atomic_bool   transformed_;
    VectorComposite    transformation_;
    mutable std::mutex mut_transform_;
 
    LoaderPtr loader_;
 
  private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
    Problem(SizeEigen            _dim,
            const TypeComposite& _frame_types = {},
            LoaderPtr            _loader      = nullptr);  // USE create() below !!
    void setup();
 
  public:
    static ProblemPtr create(SizeEigen            _dim,
                             const TypeComposite& _frame_types = {},
                             LoaderPtr            _loader      = nullptr);  // USE THIS AS A CONSTRUCTOR!
    static ProblemPtr autoSetup(const std::string&       _input_yaml_file,
                                std::vector<std::string> _primary_schema_folders = {},
                                LoaderPtr                _loader                 = nullptr,
                                bool                     _skip_yaml_validation   = false);
    static ProblemPtr autoSetup(YAML::Node               _param_node,
                                std::vector<std::string> _primary_schema_folders = {},
                                LoaderPtr                _loader                 = nullptr,
                                bool                     _skip_yaml_validation   = false);
    virtual ~Problem();
 
    // Properties -----------------------------------------
    LoaderPtr     getLoader();
    SizeEigen     getDim() const;
    TypeComposite getFrameTypes(StateKeys _keys) const;
 
  protected:
    void addFrameTypes(const TypeComposite& _types);
 
    // Tree manager --------------------------------------
  public:
    void                    setTreeManager(TreeManagerBasePtr _gm);
    TreeManagerBaseConstPtr getTreeManager() const;
    TreeManagerBasePtr      getTreeManager();
 
    // Hardware branch ------------------------------------
    HardwareBaseConstPtr getHardware() const;
    HardwareBasePtr      getHardware();
 
    SensorBasePtr installSensor(YAML::Node _sensor_node, std::vector<std::string> _schema_folders = {});
 
    SensorBasePtr installSensor(const std::string& _params_yaml_filename, std::vector<std::string> _schema_folders);
 
    ProcessorBasePtr installProcessor(YAML::Node _processor_node, std::vector<std::string> _schema_folders = {});
 
    ProcessorBasePtr installProcessor(SensorBasePtr            _sensor_corresponding,
                                      const std::string&       _params_yaml_filename,
                                      std::vector<std::string> _schema_folders);
 
    ProcessorBasePtr installProcessor(const std::string&       _params_yaml_filename,
                                      std::vector<std::string> _schema_folders);
 
    SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
    SensorBasePtr      findSensor(const std::string& _sensor_name);
 
    ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
    ProcessorBasePtr      findProcessor(const std::string& _processor_name);
 
  protected:
    void addMotionProvider(MotionProviderPtr _processor_motion_ptr);
    void removeMotionProvider(MotionProviderPtr proc);
 
  public:
    std::map<unsigned int, MotionProviderConstPtr> getMotionProviderMap() const;
    std::map<unsigned int, MotionProviderPtr>      getMotionProviderMap();
 
    // Trajectory branch ----------------------------------
    TrajectoryBaseConstPtr getTrajectory() const;
    TrajectoryBasePtr      getTrajectory();
 
    // First Frame
    bool         isFirstFrameSpecified() const;
    bool         isFirstFrameApplied() const;
    void         setFirstFrameOptions(const YAML::Node& _first_frame_node);
    void         setFirstFrameOptions(const TypeComposite&   _first_frame_types,
                                      const VectorComposite& _first_frame_values,
                                      const PriorComposite&  _first_frame_priors);
    FrameBasePtr applyFirstFrameOptions(const TimeStamp& _ts);
    FrameBasePtr emplaceFirstFrame(const TimeStamp& _ts, const YAML::Node& _first_frame_node);
    FrameBasePtr emplaceFirstFrame(const TimeStamp&       _ts,
                                   const TypeComposite&   _first_frame_types,
                                   const VectorComposite& _first_frame_values,
                                   const PriorComposite&  _first_frame_priors);
 
    FrameBasePtr emplaceFrame(const TimeStamp&       _time_stamp,
                              const TypeComposite&   _frame_types,
                              const VectorComposite& _frame_state,
                              const PriorComposite&  _frame_priors = {});
 
    FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys);
 
    FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const VectorComposite& _frame_state);
 
    FrameBasePtr emplaceFrame(const TimeStamp&       _time_stamp,
                              const StateKeys&       _frame_keys,
                              const Eigen::VectorXd& _frame_state);
 
    void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _frame_keys);
 
    void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const VectorComposite& _frame_states);
 
    // Frame getters
    FrameBaseConstPtr getLastFrame() const;
    FrameBasePtr      getLastFrame();
    FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
    FrameBasePtr      closestFrameToTimeStamp(const TimeStamp& _ts);
 
    bool permitKeyFrame(ProcessorBaseConstPtr _processor_ptr) const;
 
    void keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr);
 
    // State getters
    TimeStamp       getTimeStamp() const;
    VectorComposite getState(const StateKeys& _keys) const;
    VectorComposite getState(const TimeStamp& _ts, const StateKeys& _keys) const;
    VectorComposite getOdometry(const StateKeys& _keys) const;
 
    // Zero state provider
    VectorComposite stateZero(TypeComposite _types) const;
    VectorComposite stateZero(const StateKeys& _keys) const;
 
    // Map branch -----------------------------------------
    MapBaseConstPtr getMap() const;
    MapBasePtr      getMap();
    void            setMap(MapBasePtr);
    bool            saveMap(const std::string& _filename_dot_yaml,
                            const std::string& _map_name = "Map automatically saved by Wolf");
 
    // All branches -------------------------------------------
    void perturb(double amplitude = 0.01);
    // transform states
    void            transform(const VectorComposite& _transformation);
    bool            isTransformed() const;
    void            resetTransformed();
    VectorComposite getTransformation() const;
 
    // Covariances
    void clearCovariance();
    void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
    void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
    bool getCovarianceBlock(StateBlockConstPtr _state1,
                            StateBlockConstPtr _state2,
                            Eigen::MatrixXd&   _cov,
                            const int          _row = 0,
                            const int          _col = 0) const;
    bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
    bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
    bool getCovariance(NodeStateBlocksConstPtr _has_states_ptr,
                       const StateKeys&        _keys,
                       Eigen::MatrixXd&        _covariance) const;
    bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
 
    // Solver management ----------------------------------------
 
    StateBlockPtr notifyStateBlock(StateBlockPtr _state_ptr, Notification _notif);
 
    SizeStd getStateBlockNotificationMapSize() const;
 
    bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
 
    FactorBasePtr notifyFactor(FactorBasePtr _factor_ptr, Notification _notif);
 
    SizeStd getFactorNotificationMapSize() const;
 
    bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
 
  protected:
    void consumeNotifications(std::map<StateBlockPtr, Notification>&, std::map<FactorBasePtr, Notification>&);
 
    std::map<StateBlockPtr, Notification> consumeStateBlockNotificationMap();
 
    std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
 
  public:
    // Print and check ---------------------------------------
    void print(int           depth        = 4,      //
               bool          factored_by  = false,  //
               bool          metric       = true,   //
               bool          state_blocks = false,
               std::ostream& stream       = std::cout) const;
    bool check(int verbose_level = 0) const;
    bool check(bool verbose, std::ostream& stream) const;
};
 
}  // namespace wolf
 
// IMPLEMENTATION
 
namespace wolf
{
inline LoaderPtr Problem::getLoader()
{
    return loader_;
}
 
inline TreeManagerBaseConstPtr Problem::getTreeManager() const
{
    return tree_manager_;
}
 
inline TreeManagerBasePtr Problem::getTreeManager()
{
    return tree_manager_;
}
 
inline bool Problem::isFirstFrameSpecified() const
{
    return first_frame_status_ == 1 || first_frame_status_ == 2;
}
 
inline bool Problem::isFirstFrameApplied() const
{
    return first_frame_status_ == 2;
}
 
inline std::map<unsigned int, MotionProviderConstPtr> Problem::getMotionProviderMap() const
{
    std::map<unsigned int, MotionProviderConstPtr> map_const;
    for (auto&& pair : motion_provider_map_) map_const[pair.first] = pair.second;
    return map_const;
}
 
inline std::map<unsigned int, MotionProviderPtr> Problem::getMotionProviderMap()
{
    return motion_provider_map_;
}
 
inline SizeStd Problem::getStateBlockNotificationMapSize() const
{
    std::lock_guard<std::mutex> lock(mut_notifications_);
    return state_block_notification_map_.size();
}
 
inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
{
    std::lock_guard<std::mutex> lock(mut_notifications_);
    return factor_notification_map_.size();
}
 
inline void Problem::consumeNotifications(std::map<StateBlockPtr, Notification>& _sb_notification_map,
                                          std::map<FactorBasePtr, Notification>& _fac_notification_map)
{
    std::lock_guard<std::mutex> lock(mut_notifications_);
 
    _sb_notification_map  = std::move(state_block_notification_map_);
    _fac_notification_map = std::move(factor_notification_map_);
}
 
}  // namespace wolf